/*
 * Copyright (C) 2006 Roman Krashanitsa
 *
 * @author Roman Krashanitsa
 *
 * This file is part of paparazzi.
 *
 * paparazzi is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2, or (at your option)
 * any later version.
 *
 * paparazzi is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with paparazzi; see the file COPYING.  If not, write to
 * the Free Software Foundation, 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA. 
 *
 */

package pprz;

import java.io.IOException;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.util.HashMap;

import javax.comm.UnsupportedCommOperationException;
import javax.xml.parsers.ParserConfigurationException;
import javax.xml.parsers.SAXParser;
import javax.xml.parsers.SAXParserFactory;

import org.xml.sax.helpers.DefaultHandler;

import pprz.data_components.Aircraft;
import pprz.data_components.Message;
import pprz.data_components.PprzMsgProtocol;
import pprz.data_components.XBee;
import pprz.data_components.XBeeProtocol;
import pprz.data_components.events.DataEvent;
import pprz.data_components.events.DownlinkMessageEvent;
import pprz.data_components.events.DownlinkMessageEventListener;
import pprz.data_components.parsers.ConfParser;

import fr.dgac.ivy.Ivy;
import fr.dgac.ivy.IvyClient;
import fr.dgac.ivy.IvyException;
import fr.dgac.ivy.IvyMessageListener;


public class Link_test implements IvyMessageListener, DownlinkMessageEventListener {

	private Ivy bus;
	private DefaultHandler conf ;
	private XBee xbee;
	private XBeeProtocol xbeeprot;
	private PprzMsgProtocol decoder;
	private HashMap<Integer,Aircraft> aircrafts;	
	
	Link_test() throws UnsupportedCommOperationException, IOException, IvyException {
		// initialization, name and ready message
		bus = new Ivy("Link","Link ready",null);
		// classical subscription
		bus.bindMsg("^Hello(.*)",this);
		// inner class subscription ( think awt )
		bus.bindMsg("^Bye$",new IvyMessageListener() {
			public void receive(IvyClient client, String[] args) {
				// leaves the bus, and as it is the only thread, quits
				bus.stop();
			}
		});
		
		byte cc[];
		cc=new byte[4];
		cc[0]=32; cc[1]=33; cc[2]=34; cc[3]=35;
		for (int i=0; i<cc.length; i++) System.out.println(cc[i]); 
		
		// create and initialize modem object
		xbee = new XBee("COM1",100,"Ground Station");
		if (!(xbee.ok)) {
			System.out.println("XBee modem is not responding.. exiting.");
			System.exit(0);
		}
	//	xbee.setDestNode(5001);
		xbee.enableAPI();
	//	xbee.enableRTS();
		// create and initialize protocol object
		aircrafts=new HashMap<Integer,Aircraft>(5);
//		decoder=new PprzMsgDecoder(aircrafts);		
//		xbeeprot = new XBeeProtocol(decoder);
		//bind devices and protocols
//		xbee.addDataEventListener(xbeeprot);			
//		xbeeprot.addDownlinkMessageEventListener(this);
		
		
		
		
		SAXParser parser;
		conf = new ConfParser();
		try {
			SAXParserFactory prt=SAXParserFactory.newInstance();
			parser=prt.newSAXParser();
			parser.parse("C:\\Roman\\Development\\Java\\groundStation\\pprz\\conf.xml", conf);
		} catch (org.xml.sax.SAXException se) {
			System.out.println("can't create parser");
		} catch (ParserConfigurationException se) {
			System.out.println("can't create parser");
		} catch (IOException ex) {
			System.out.println("IO problem");
		}
		bus.start(null); // starts the bus on the default domain
	}
	
	// callback associated to the "Hello" messages"
	public void receive(IvyClient client, String[] args) {
		try {
			bus.sendMsg("Bonjour"+((args.length>0)?args[0]:""));
		} catch (IvyException ie) {
			System.out.println("can't send my message on the bus");
		}
	}

	public void dataEvent(DataEvent ev)
	{
//		System.out.println("New data available:");
		byte[] cc=ev.getData();
		int len=cc.length;
		System.out.printf("RF Data:\tFROM=%d\tRSSI=%d\tlen=%d\n", 
						  xbee.lastSender, xbee.RSSI, len);
		
		for (int i=0; i<len; i++) 
			System.out.printf("\t%d",((int)(cc[i]) & 0xFF));
		System.out.println();
		
	}
	
	public void messageEvent(DownlinkMessageEvent ev) {
		Message m=ev.getMessage();
		short id;
		float vel, pitch, roll, yaw, alt;
		id=m.ac(); 
		byte[] cc=m.payload();
		switch (m.id())
		{
		case 6:
		{
			ByteBuffer b=ByteBuffer.wrap(cc, 2, 6);
			b.order(ByteOrder.LITTLE_ENDIAN);
			roll=b.getShort();
			yaw=b.getShort();
			pitch=b.getShort();
			System.out.printf("roll=%f\tyaw=%f\tpitch=%f\n",roll,yaw,pitch);
		}
		case 8:
		{}
		case 11:
		{}
		case 12:
		{}
		case 23:
		{}	
		}
	}
	
	public static void main(String args[]) 
	throws UnsupportedCommOperationException, IvyException 
	{
		try {
			new Link_test();
		} catch (IOException e)	{
			System.out.println("Modem error..");
		}
	}
}

